/*
 * L3G4200D.c
 *
 */

#include "L3G4200D.h"
#include "stm32f10x.h"
#include "i2c.h"
#include "tool.h"
#include <stdlib.h>
#include <stdio.h>
#include "memory.h"
#include "Console.h"
#include "string.h"


#define L3G4200D_ID		0xD3

#define L3G4200D_SAD_W		0xD2
#define L3G4200D_SAD_R		0xD3

#define MULTI_RW			0x80


#define Who_Am_I					0x0F
#define Ctrl_Reg1					0x20
#define Ctrl_Reg2					0x21
#define Ctrl_Reg3					0x22
#define Ctrl_Reg4					0x23
#define Ctrl_Reg5					0x24
#define Reference					0x25
#define Out_Temp					0x26
#define Status_Reg					0x27
#define Out_X_L						0x28
#define Out_X_H						0x29
#define Out_Y_L						0x2A
#define Out_Y_H						0x2B
#define Out_Z_L						0x2C
#define Out_Z_H						0x2D
#define FIFO_Ctrl_Reg				0x2E
#define FIFO_SRC_Reg				0x2F
#define INT1_CFG					0x30
#define INT1_SRC					0x31
#define INT1_TSH_XH					0x32
#define INT1_TSH_XL					0x33
#define INT1_TSH_YH					0x34
#define INT1_TSH_YL					0x35
#define INT1_TSH_ZH					0x36
#define INT1_TSH_ZL					0x37
#define INT1_Duration				0x38


typedef struct _LIS_INIT_TBL{
	unsigned char reg;
	unsigned char value;
}LIS_INIT_TBL;

const LIS_INIT_TBL L3G4200D_Init_Tbl[] =
{
	//A
	{ Ctrl_Reg1,			0x7F },
	{ Ctrl_Reg2,			0x24 },
	{ Ctrl_Reg3,			0x00 },
	{ Ctrl_Reg4,			0x00 },	//0x30 },
	{ Ctrl_Reg5,			0x00 },
	{ FIFO_Ctrl_Reg,		0x00 },
	{ INT1_CFG,				0x00 },
	{ INT1_TSH_XH, 			0x00 },
	{ INT1_TSH_XL, 			0x00 },
	{ INT1_TSH_YH, 			0x00 },
	{ INT1_TSH_YL, 			0x00 },
	{ INT1_TSH_ZH, 			0x00 },
	{ INT1_TSH_ZL, 			0x00 },
	{ INT1_Duration,		0x00 },
};


static int L3G4200D_Read(unsigned char reg, char *buf, unsigned char len)
{
	if (len > 1)
		reg |= MULTI_RW;
	
	return I2C_Read(L3G4200D_SAD_R, reg, (unsigned char *)buf, len);
}

/**********
static int L3G4200D_Write(unsigned char reg, char *buf, unsigned char len)
{
	if (len > 1)
		reg |= MULTI_RW;
	
	return I2C_Write(L3G4200D_SAD_W, reg, (unsigned char *)buf, len);
}
***********/


int L3G4200D_Init(void)
{
	int i;
	
	I2C_HW_Init();
    	
	for (i = 0; i < (sizeof(L3G4200D_Init_Tbl)/sizeof(LIS_INIT_TBL)); i++)
	{
		I2C_Write(L3G4200D_SAD_W, L3G4200D_Init_Tbl[i].reg, (unsigned char *)&L3G4200D_Init_Tbl[i].value, 1);
	}

	return 0;
}

void L3G4200D_GET_XYZ(short *arg)
{
	char buf[7];
	
	L3G4200D_Read(Status_Reg, buf, 7);
	
	arg[0] = (((short)buf[2]) << 8) | (buf[1] & 0x00FF);
	arg[1] = (((short)buf[4]) << 8) | (buf[3] & 0x00FF);
	arg[2] = (((short)buf[6]) << 8) | (buf[5] & 0x00FF);
	
}

unsigned char L3G4200D_GET_ID(void)
{	
	char buf[2];
	L3G4200D_Read(Who_Am_I, buf, 2);
	return (unsigned char)buf[0];
}

char L3G4200D_GET_TEMPERATURE(void)
{	
	char buf[2];
	L3G4200D_Read(Out_Temp, buf, 2);
	return (unsigned char)buf[0];
}

